#include "dynamic_routing.h"

// 全局变量
std::vector<myObstacle> obstacles_ptr;
std::vector<Obstacle> AllObstacle;                        // 感知一帧识别到的所有障碍物
std::pair<vector<double>, vector<double>> reference_path; // 参考路径点位置（x,y）
geometry_msgs::Pose start_pose_;                          // 起点
geometry_msgs::Pose goal_pose_;                           // 终点
nav_msgs::Odometry sub_odom_;                             // odom
bool messege1;                                            // 控制发布一次的变量
bool messege2;                                            // 控制发布一次的变量
double init_lon_state;                                    // 初始弧长
std::vector<double> accumulated_s;                        // 本参考线纵向距离
std::vector<ReferencePoint> reference_points;             // 本参考线参考路径点参数

//-------------------------------------------回调函数---------------------------------------------//
/*定义起点位置*/
void Dynamic_routing::start_pose_call_backs(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
  if (ego_is_running == false)
  {
    start_pose_.position.x = msg->pose.pose.position.x;
    start_pose_.position.y = msg->pose.pose.position.y;
    start_pose_.orientation = msg->pose.pose.orientation;
    theta_init = tf2::getYaw(msg->pose.pose.orientation); // 初始朝向
  }else{
    ROS_ERROR("start_pose_call_backs function !ego_is_running = false.");
  }
}

/*定义终点位置*/
void Dynamic_routing::goal_pose_call_backs(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  if (ego_is_running == false)
  {
        // -------------------------路径规划参数在定义终点的时候初始化--------------------------//
    d0 = 0;                           // 初始的横向偏移值 [m]
    dd0 = 0;                          // 初始的横向速度 [m/s]
    ddd0 = 0;                         // 初始的横向加速度 [m/s^2]
    init_lon_state = 0;               // 初始的纵向值[m]
    ds0 = vs0;                          // 初始的纵向速度[m/s]
    dds0 = 0;                         // 初始的纵向加速度[m/ss]
    init_relative_time = 0;           // 规划起始点的时间
    messege1 = false;
    goal_pose_.position.x = msg->pose.position.x;
    goal_pose_.position.y = msg->pose.position.y;
    theta_end = tf2::getYaw(msg->pose.orientation);
  }else{
    ROS_ERROR("goal_pose_call_backs function !ego_is_running = false.");
  }
}

/*订阅获取地图坐标*/
void Dynamic_routing::path_pose_call_backs(const nav_msgs::Path &path_point_)
{
  reference_path.first.clear();
  reference_path.second.clear();
  accumulated_s.clear();
  reference_points.clear();
  if (path_point_.poses.size() > 0)
  {
    std::vector<double> headings;
    std::vector<double> kappas;
    std::vector<double> dkappas;

    for (size_t i = 0; i < path_point_.poses.size(); i++)
    {
      reference_path.first.push_back(path_point_.poses[i].pose.position.x);
      reference_path.second.push_back(path_point_.poses[i].pose.position.y);
      std::pair<double, double> xy;
      xy.first = path_point_.poses[i].pose.position.x;
      xy.second = path_point_.poses[i].pose.position.y;
      xy_points.push_back(xy);
    }
    if (!PathMatcher::ComputePathProfile(xy_points, &headings, &accumulated_s, &kappas, &dkappas))
    {
      ROS_WARN("rerferenceline generate failed!");
    }
    else
    {
      for (size_t i = 0; i < xy_points.size(); i++)
      {
        // 创建ReferencePoint类
        ReferencePoint reference_point(kappas[i], dkappas[i], xy_points[i].first, xy_points[i].second, headings[i],
                                       accumulated_s[i]);
        reference_points.emplace_back(reference_point);
      }
      ROS_WARN("rerferenceline generate successfully!");
    }
    csp = new CubicSpline2D(reference_path.first, reference_path.second, accumulated_s); // 只需要执行一次
    // 初始化
    kappa_init = kappas.front();
    dkappa_init = dkappas.front();
    init_lon_state = 0; // 复位
  }else{
    ROS_ERROR("!path_point_.poses.size() > 0");
  }
  set_reference = true;
}

/*读回ros odom坐标系数据 , 接收车的里程信息，控制车的移动*/
void Dynamic_routing::odom_call_back(const nav_msgs::Odometry &odom)
{
  sub_odom_ = odom; // 车的里程信息，就是位置信息
  double x = odom.pose.pose.position.x;
  double y = odom.pose.pose.position.y;
  double yaw = tf::getYaw(odom.pose.pose.orientation);

  // 计算四个顶点
  corners.clear();

  for (auto &corner : std::vector<std::pair<double, double>>{
          {half_length, half_width},
          {half_length, -half_width},
          {-half_length, -half_width},
          {-half_length, half_width}}) {
    geometry_msgs::Point p;
    p.x = x + corner.first * cos(yaw) - corner.second * sin(yaw);
    p.y = y + corner.first * sin(yaw) + corner.second * cos(yaw);
    p.z = 0;
    corners.push_back(p);
  }

  // 创建Marker
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time::now();
  marker.ns = "vehicle";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.1; // 线条宽度

  // 车辆轮廓颜色
  marker.color.r = 1.0;
  marker.color.g = 0.0;
  marker.color.b = 0.0;
  marker.color.a = 1.0;
  // 添加顶点
  for (const auto &corner : corners) {
    marker.points.push_back(corner);
  }
    // 连接最后一个点到第一个点
    marker.points.push_back(corners.front());

  vehicle_outline_pub_.publish(marker);
}

// 计算向量的点积
double Dynamic_routing::dotProduct(const xy_pose& a, const xy_pose& b) {
  return a.x * b.x + a.y * b.y;
}

// 计算向量的模长
double Dynamic_routing::magnitude(const xy_pose& a) {
  return sqrt(dotProduct(a, a));
}

// 计算夹角（单位：弧度）
double Dynamic_routing::angleBetween(const xy_pose& A, const xy_pose& B, const xy_pose& C) {
  // 计算向量 AB 和 AC
  xy_pose AB = {B.x - A.x, B.y - A.y};
  xy_pose AC = {C.x - A.x, C.y - A.y};

  // 计算点积和模长
  double dot = dotProduct(AB, AC);
  double magAB = magnitude(AB);
  double magAC = magnitude(AC);

  // 计算余弦值
  double cosTheta = dot / (magAB * magAC);

  // 限制在 [-1, 1] 范围内，避免数值误差导致的错误
  if (cosTheta < -1.0) cosTheta = -1.0;
  if (cosTheta > 1.0) cosTheta = 1.0;

  // 计算并返回夹角（弧度）
  return acos(cosTheta);
}

//------------------------------------------子函数---------------------------------------------//
// 判断车是否走完轨迹的设置距离，是的话再发给局部规划，在车目前的位置之前更新局部规划轨迹
int Dynamic_routing::is_update_dynamic(nav_msgs::Path &trj_point_array, nav_msgs::Odometry &odom, int size)
{
  double distance = sqrt(pow(odom.pose.pose.position.x - trj_point_array.poses[size].pose.position.x, 2) +
                        pow(odom.pose.pose.position.y - trj_point_array.poses[size].pose.position.y, 2));
  if (distance < 1.0) // 接近了
  {
    return 1;
  }
  int end_size = round((trj_point_array.poses.size() * 2) / 3);
  xy_pose end_pose, odom_pose, end_next_pose;
  end_pose.x = trj_point_array.poses[end_size].pose.position.x;
  end_pose.y = trj_point_array.poses[end_size].pose.position.y;
  end_next_pose.x = trj_point_array.poses[end_size + 1].pose.position.x;
  end_next_pose.y = trj_point_array.poses[end_size + 1].pose.position.y;
  odom_pose.x = odom.pose.pose.position.x;
  odom_pose.y = odom.pose.pose.position.y;

  double angle = angleBetween(end_pose, end_next_pose, odom_pose);
  double angleInDegrees = angle * (180.0 / M_PI);
  if (angleInDegrees < 90) // 接近了
  {
    return 2;
  }
  return 0;
}

bool Dynamic_routing::is_collision(FrenetPath *fts, double COLLISION_CHECK)
{
  // 障碍物的避障
  for (const auto& obstacle_ptr : obstacles_ptr) // 遍历障碍物
  {
    for (size_t j = 0; j < fts->x.size() - 1; j++) // 后半部分的点
    {
      double llx = obstacle_ptr.x;
      double lly = obstacle_ptr.y;
      double xx = llx - fts->x[j];
      double yy = lly - fts->y[j];
      double x2 = xx * xx;

      double y2 = yy * yy;
      double closest1 = sqrt(x2 + y2);
      if (closest1 <= COLLISION_CHECK) // 发现有点距离障碍物很近
      {
        return true;
        break;
      }
    }
  }
  return false;
}

//---------------------------------默认构造函数：规划函数,初始化参数---------------------------------//
Dynamic_routing::Dynamic_routing(void)
{
  ros::NodeHandle n;
  n.getParam("carla_simulation", carla_simulation);
  n.getParam("MaxT", MaxT);
  n.getParam("MinT", MinT);
  n.getParam("goal_distanse", goal_distanse);
  n.getParam("update_pos", update_pos);
  n.getParam("left_d", left_d);
  n.getParam("right_d", right_d);
  n.getParam("target_speed", target_speed);
  n.getParam("vs0", vs0);
  n.getParam("tDelta", tDelta);
  n.getParam("dDelta", dDelta);
  n.getParam("vehicle_width", vehicle_width);
  n.getParam("vehicle_length", vehicle_length);

  half_width = vehicle_width / 2.0;
  half_length = vehicle_length / 2.0;
  


  ////////////////////////////////发布////////////////////////////////////////
  waypoints_vis_pub_ = n.advertise<nav_msgs::Path>("/xyl/planning/dynamic_waypoints_vis", 10);       // 发布局部轨迹
  waypoints_pub_ = n.advertise<waypoint_msgs::WaypointArray>("/xyl/planning/dynamic_waypoints", 10); // 发布局部轨迹

  local_paths_a = n.advertise<geometry_msgs::PoseArray>("/xyl/planning/dynamic_paths_a", 10); // 发布局部轨迹的加速度
  local_paths_t = n.advertise<geometry_msgs::PoseArray>("/xyl/planning/dynamic_paths_t", 10); // 发布局部轨迹的航向角
  local_paths_k = n.advertise<geometry_msgs::PoseArray>("/xyl/planning/dynamic_paths_k", 10); // 发布局部轨迹的曲率
  Start_Dynamic = n.advertise<std_msgs::String>("/xyl/planning/Start_Dynamic", 10);           // 发布局部轨迹成功生存的信号
  
  node_kill = n.advertise<std_msgs::String>("/xyl/node_kill", 10);       //发布杀死节点信号
  vehicle_outline_pub_ = n.advertise<visualization_msgs::Marker>("/vehicle_outline", 10);

  start_pose_subscriber_ = n.subscribe("/initialpose", 10, &Dynamic_routing::start_pose_call_backs, this);         // 订阅
  goal_pose_subscriber_ = n.subscribe("/move_base_simple/goal", 10, &Dynamic_routing::goal_pose_call_backs, this); // 订阅

  path_pose_subscriber_ = n.subscribe("/xyl/refer_Point", 1000, &Dynamic_routing::path_pose_call_backs, this); // 订阅全局路径，为局部服务
  odom_sub_ = n.subscribe("/odom", 10, &Dynamic_routing::odom_call_back, this);
  // carla停车场联合仿真的时候，不需要参考线信息

  // 打印规划器

  ROS_WARN("Planner: frenet based");
  // 参数初始化
  best_ds0 = 0.0;
  ctf_s  = 0.0;
  ctf_d = 0.0;
  ego_is_running = false;
  set_reference = false;
  is_vehicle_obstacle = true;

  string aa = "0";
  start_dynamic.data = aa.c_str();
  Start_Dynamic.publish(start_dynamic);

  routing_thread_ = new boost::thread(boost::bind(&Dynamic_routing::thread_routing, this));
  obs_thread_ = std::make_unique<std::thread>(&Dynamic_routing::ObsThread,this);
}

void Dynamic_routing::ObsThread(void)
{
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    if(!corners.empty())
    {
      std::vector<myObstacle> obstacle = obs.getObstaclesAsPointers();
      for(const auto& obsta : obstacle)
      {
        for(const auto& corner : corners)
        {
          double view_dis = sqrt(pow(corner.x - obsta.x, 2) + pow(corner.y - obsta.y, 2));
          if (view_dis < 0.15) 
          {
            is_vehicle_obstacle = false;
            ROS_INFO("Obstacle too close: Distance = %f", view_dis);
            break;
          }
        }
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

Dynamic_routing::~Dynamic_routing(void)
{
  delete routing_thread_;
  delete best_frenet_path;
  delete csp;
}

void Dynamic_routing::thread_routing(void)
{
  ros::NodeHandle n;
  ros::Rate loop_rate(20);
  // 类实例化对象
  FrenetOptimalTrajectory fot;
  
  // -------------------------Frenet采样路径规划参数初始化--------------------------//
  FrenetHyperparameters fot_hp = {
      3.6 / 3, // 最大速度 [m/s]，纵向和横向速度的尺量合
      2.0,        // 最大加速度[m/ss]
      10,          // 最大曲率 [1/m]
      left_d,          // 最大道路宽度 [m]
      right_d,         // 最小道路宽度 [m]
      dDelta,        // 道路宽度采样间隔 [m]，值越小，轨迹好的越多，但是太小计算效率不行
      tDelta,        // 时间采样间隔[s]，值越大生成的轨迹速度越快
      MaxT,       // 最大预测时间 [s]，值越大生成的轨迹距离越远
      MinT,       // 最小预测时间 [s]，值越大生成的轨迹长度越长
      0.1,        // 目标速度采样间隔 [m/s]
      1,          // 目标速度的采样数量
      0.2,        // 目标位置采样间隔 [m/s]
      1,          // 目标位置的采样数量

      // 损失函数权重
      5.0, // Distance from reference path
      5.0, // Target speed
      1.0, // Target acceleration
      1.0, // Jerk
      0.5, // time
      // 总的
      1, // Lateral
      1, // Longitudin
  };

  while (n.ok())
  {
    if (set_reference == true && is_vehicle_obstacle == true)
    {
      //--------------------------------------------------生成轨迹--------------------------------------------------//
      if (reference_points.size() > 0 ||  reference_points.size() == 0) 
      {
        obstacles_ptr.clear();
        obstacles_ptr = obs.getObstaclesAsPointers();
        // std::cout << "obstacles_ptr.size(): " << obstacles_ptr.size() << std::endl;
        /* for (const auto& obstacle_ptr : obstacles_ptr) {
          std::cout << "Obstacle: x = " << obstacle_ptr->x << ", y = " << obstacle_ptr->y << std::endl;
        } */
        // -----------------------------------------轨迹规划方案选择执行-------------------------------------------//
        // //车没有合适轨迹停下来重新规划的时候， 有些路径规划参数重新初始化
        if (messege2 == true)
        {
          // 有些路径规划参数重新初始化
          ds0 = best_ds0; // 初始的纵向速度[m/s]
          init_relative_time = 0;           // 规划起始点的时间
        }
        // auto beforeTime = std::chrono::steady_clock::now(); //计时开始

        best_frenet_path = nullptr; // 确保已初始化最佳路径
        std::priority_queue<FrenetPath *, std::vector<FrenetPath *>, CostComparator_for_Frenet> frenet_paths;
        FrenetInitialConditions fot_ic = {
            init_lon_state,               // 初始的纵向值[m]
            ds0,                          // 初始的纵向速度[m/s]
            dds0,                         // 初始的纵向加速度[m/ss]
            d0,                           // 初始的横向偏移值 [m]
            dd0,                          // 初始的横向速度 [m/s]
            ddd0,                         // 初始的横向加速度 [m/s^2]
            0.0,                          // 目标横向速度配置
            0.0,                          // 目标横向加速度配置
            target_speed, // 目标速度（即纵向的速度保持） [m/s]
            3,                           // 目标位置,就是前探距离，测试3，按理说要变量赋值
            reference_path.first,         // 输入的参考线x坐标数组
            reference_path.second,        // 输入的参考线y坐标数组
        };
        // 计算轨迹，返回通过各种检测的轨迹数

        fot.calc_frenet_paths(&fot_ic, &fot_hp, frenet_paths, csp, reference_path.first, reference_path.second);
        // 筛选最优轨迹
        std::vector<FrenetPath*> valid_paths;
        double threshold_levels[] = {0.3, 0.25, 0.2};  // 定义不同的碰撞检测阈值
        int selected_path_index = 0; // 记录选中的路径索引
        double selected_threshold = 0; // 记录选中的阈值

        auto path_select_start = std::chrono::steady_clock::now();

        while (!frenet_paths.empty()) {
            auto top = frenet_paths.top();
            frenet_paths.pop();

            for (double threshold : threshold_levels) {
                if (!is_collision(top, threshold)) {
                    valid_paths.push_back(top);  // 保存通过当前阈值的路径
                      if (selected_path_index == 0) { // 只记录第一个符合条件的路径
                        selected_path_index = valid_paths.size();
                        selected_threshold = threshold;
                      }
                    break;  // 如果当前路径满足某个阈值，跳出循环，不再检查更低阈值
                }
            }

            // 如果路径没有被保存，意味着它未通过任何一个阈值的检测，需要删除它
            if (std::find(valid_paths.begin(), valid_paths.end(), top) == valid_paths.end()) {
                delete top;
            }
        }

        // 优先选择满足较高阈值的路径
        for (double threshold : threshold_levels) {
            for (auto path : valid_paths) {
                if (!is_collision(path, threshold)) {
                    best_frenet_path = path;
                    break;
                }
            }
            if (best_frenet_path != nullptr) {
                break;  // 如果找到了合适的路径，立即跳出
            }
        }

        // 清理未选中的路径
        for (auto path : valid_paths) {
            if (path != best_frenet_path) {
                delete path;
            }
        }
        // auto path_select_end = std::chrono::steady_clock::now();
        // double path_select = std::chrono::duration<double, std::milli>(path_select_end - path_select_start).count();
        // std::cout << path_select << "ms" << std::endl;

        best_path = fot.FrenetPath_to_TrajectoryPoint(best_frenet_path);

        /* auto afterTime = std::chrono::steady_clock::now(); //计时结束
        double duration_millsecond = std::chrono::duration<double, std::milli>(afterTime - beforeTime).count();
        std::cout << duration_millsecond << "ms" << std::endl; */
        // std::cout << "best_path:" << best_path.size() << "\n";

        // --------------------------------------最优轨迹的逻辑处理--------------------------------------//
        messege2 = false;
        if (best_path.size() == 0 || is_vehicle_obstacle == false) // 没有轨迹数量
        {
          if (messege2 == false) // 只发布一次
          {
            ROS_WARN("vehical can not pass the road!");
            string aa = "2";
            start_dynamic.data = aa.c_str();
            Start_Dynamic.publish(start_dynamic); // 没有轨迹，发送停车信号
            // 发布空轨迹
            traj_points_.poses.clear();
            traj_points_.header.frame_id = Frame_id;
            traj_points_.header.stamp = ros::Time::now();
            waypoints_vis_pub_.publish(traj_points_);
            waypoint_msgs::WaypointArray local_waypoints;
            local_waypoints.header.frame_id = Frame_id;
            waypoints_pub_.publish(local_waypoints);
            messege1 = false; // 复位
            messege2 = true;  // 复位
          }
        }
        else
        {
          //---------------------------------发布轨迹---------------------------------//
          // 复制给traj_points_，给发布
          traj_points_.poses.clear();
          traj_points_.header.frame_id = Frame_id;
          traj_points_.header.stamp = ros::Time::now();
          waypoint_msgs::WaypointArray local_waypoints;
          local_waypoints.header.frame_id = Frame_id;
          for (int i = 0; i < best_path.size(); i++)
          {
            geometry_msgs::PoseStamped pose_stamp;
            pose_stamp.header.frame_id = Frame_id;
            pose_stamp.header.stamp = ros::Time::now();
            pose_stamp.pose.position.x = best_path[i].x;
            pose_stamp.pose.position.y = best_path[i].y;
            pose_stamp.pose.position.z = 0;
            traj_points_.poses.push_back(pose_stamp);
            //
            waypoint_msgs::Waypoint point;
            point.pose.pose.position.x = best_path[i].x;
            point.pose.pose.position.y = best_path[i].y;
            point.twist.twist.linear.x = best_path[i].v;
            local_waypoints.waypoints.push_back(point);
          }
          waypoints_pub_.publish(local_waypoints);
          waypoints_vis_pub_.publish(traj_points_);

          //--------------------------------发布加速度---------------------------------//
          pubLocalPath_a.poses.clear();
          pubLocalPath_a.header.frame_id = Frame_id;
          pubLocalPath_a.header.stamp = ros::Time::now();
          for (size_t i = 0; i < best_path.size(); i++)
          {
            geometry_msgs::Pose init_pose;
            init_pose.position.x = best_path[i].a;
            pubLocalPath_a.poses.push_back(init_pose);
          }
          local_paths_a.publish(pubLocalPath_a);
          //--------------------------------发布角度---------------------------------//
          pubLocalPath_t.poses.clear();
          pubLocalPath_t.header.frame_id = Frame_id;
          pubLocalPath_t.header.stamp = ros::Time::now();
          for (size_t i = 0; i < best_path.size(); i++)
          {
            geometry_msgs::Pose init_pose;
            init_pose.position.x = best_path[i].theta;
            pubLocalPath_t.poses.push_back(init_pose);
          }
          local_paths_t.publish(pubLocalPath_t);
          //--------------------------------发布曲率---------------------------------//
          pubLocalPath_k.poses.clear();
          pubLocalPath_k.header.frame_id = Frame_id;
          pubLocalPath_k.header.stamp = ros::Time::now();
          for (size_t i = 0; i < best_path.size(); i++)
          {
            geometry_msgs::Pose init_pose;
            init_pose.position.x = best_path[i].kappa;
            pubLocalPath_k.poses.push_back(init_pose);
          }
          local_paths_k.publish(pubLocalPath_k);

          //--------------------------------发布局部轨迹生成成功标志---------------------------------//
          if (messege1 == false) // 只发布一次,最开始生成局部轨迹的时候发
          {
            sleep(0.3); // 等会再开车，延时1秒
            string aa = "1";
            start_dynamic.data = aa.c_str();
            Start_Dynamic.publish(start_dynamic); // 发送局部轨迹生成信号给全局路径
            messege1 = true;
          }
          //--------------------------------更新轨迹，根据位置更新---------------------------------//
          ego_is_running = true;

          if (is_update_dynamic(traj_points_, sub_odom_, update_pos + 8) == 1)
          {
            cft.cartesianToFrenet(xy_points,  sub_odom_, ctf_s, ctf_d);
            // init_lon_state =best_path[update_pos].s;
	         init_lon_state = ctf_s;
            ds0 = best_path[update_pos].s_d;
            best_ds0 = best_path[update_pos].s_d;
            dds0 = best_path[update_pos].s_dd;
            d0 = best_path[update_pos].d;
	          // d0 = ctf_d;
            dd0 = best_path[update_pos].d_d;
            ddd0 = best_path[update_pos].d_dd;

            init_relative_time = 0; // best_path[update_pos].relative_time，动态障碍物的起始时间跟这个保持一致

            theta_init = best_path[update_pos].theta;
            kappa_init = best_path[update_pos].kappa;
            dkappa_init = best_path[update_pos].dkappa;
          }else if (is_update_dynamic(traj_points_, sub_odom_, update_pos + 8) == 2){
            cft.cartesianToFrenet(xy_points,  sub_odom_, ctf_s, ctf_d);
	          init_lon_state = ctf_s;
            int end_size = round((traj_points_.poses.size() * 2) / 3);
            ds0 = best_path[end_size].s_d;
            best_ds0 = best_path[end_size].s_d;
            dds0 = best_path[end_size].s_dd;
	          d0 = ctf_d;
            dd0 = best_path[end_size].d_d;
            ddd0 = best_path[end_size].d_dd;

            init_relative_time = 0; // best_path[end_size].relative_time，动态障碍物的起始时间跟这个保持一致
            printf("2");
          }
        }
        is_vehicle_obstacle = true;
        // 到达终点判断
        if (sqrt((sub_odom_.pose.pose.position.x - goal_pose_.position.x) * (sub_odom_.pose.pose.position.x - goal_pose_.position.x) +
                 (sub_odom_.pose.pose.position.y - goal_pose_.position.y) * (sub_odom_.pose.pose.position.y - goal_pose_.position.y)) < goal_distanse)
        {
          ROS_WARN("          set_reference = false;");
          node = "2";
          nodekill.data = node.c_str();
          node_kill.publish(nodekill);
          ego_is_running = false;
          set_reference = false;
        }
      }
      else
      {
        ROS_WARN("Other reference lines are empty and cannot plan!");
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
